#include "ag_robot/PIDController.hpp"
#include <algorithm> // 用于std::clamp（C++17）

using namespace std;
// 构造函数
PIDController::PIDController(double kp, double ki, double kd, double max_output, double min_output)
    : kp_(kp), ki_(ki), kd_(kd), max_output_(max_output), min_output_(min_output),
      integral_(0.0), prev_error_(0.0) {}

// 计算控制量
double PIDController::compute(double setpoint, double actual_value) {
    double error = setpoint - actual_value;
    integral_ += error;                    // 积分项累积
    double derivative = error - prev_error_; // 微分项
    prev_error_ = error;

    // PID公式: output = Kp*e + Ki*∫e + Kd*de/dt
    double output = kp_ * error + ki_ * integral_ + kd_ * derivative;

    // 输出限幅（避免超调或执行器饱和）
    return std::clamp(output, min_output_, max_output_);
}

// 重置积分项（可选）
void PIDController::reset() {
    integral_ = 0.0;
    prev_error_ = 0.0;
}